package com.eqsoft.gesturerecognition;

import java.util.Vector;

public class Filter {

	public static final float HIGH_PASS_FACTOR = 0.1f;
	public static final float LOW_PASS_FACTOR = 0.01f;
	public static final float IDLE_SENSITIVITY = 0.1f;
	public static final float GRAVITY = 9.80665f;
	
	public static Vector<AccelVector> highPass(Vector<AccelVector> in)
	{
		Vector<AccelVector> ret = new Vector<AccelVector>();
		float[] prev = new float[] { 0, 0, 0 };
		for (int i = 0; i < in.size(); i++)
		{
			prev[0] = in.get(i).getX() * HIGH_PASS_FACTOR + prev[0] * (1 - HIGH_PASS_FACTOR);
			prev[1] = in.get(i).getY() * HIGH_PASS_FACTOR + prev[1] * (1 - HIGH_PASS_FACTOR);
			prev[2] = in.get(i).getZ() * HIGH_PASS_FACTOR + prev[2] * (1 - HIGH_PASS_FACTOR);
			
			float[] cur = new float[] { in.get(i).getX() - prev[0], in.get(i).getY() - prev[1], in.get(i).getZ() - prev[2] };
			
			ret.add(new AccelVector(cur));
		}
		
		return ret;
	}

	public static Vector<AccelVector> lowPass(Vector<AccelVector> in)
	{
		Vector<AccelVector> ret = new Vector<AccelVector>();
		float[] prev = new float[] { 0, 0, 0 };
		for (int i = 0; i < in.size(); i++)
		{
			float[] cur = new float[3];
			cur[0] = in.get(i).getX() * LOW_PASS_FACTOR + prev[0] * (1 - LOW_PASS_FACTOR);
			cur[1] = in.get(i).getY() * LOW_PASS_FACTOR + prev[1] * (1 - LOW_PASS_FACTOR);
			cur[2] = in.get(i).getZ() * LOW_PASS_FACTOR + prev[2] * (1 - LOW_PASS_FACTOR);
			prev = cur;
			
			ret.add(new AccelVector(cur));
		}
		
		return ret;
	}
	
	public static Vector<AccelVector> idle(Vector<AccelVector> in)
	{
		Vector<AccelVector> ret = new Vector<AccelVector>();
		for (int i = 0; i < in.size(); i++)
		{
			if (in.get(i).getAbs() > (1 + IDLE_SENSITIVITY) * GRAVITY || in.get(i).getAbs() < (1 - IDLE_SENSITIVITY) * GRAVITY)
				ret.add(in.get(i));
		}
		
		return ret;
	}
}
